#include "eightpoint.h"

/* -------------------------------------------------------------- */

EightPoint::EightPoint(std::vector<FeatureMatch>* _matches)
{
	this->matches = _matches;
}

/* -------------------------------------------------------------- */

EightPoint::~EightPoint()
{
}

/* -------------------------------------------------------------- */

void EightPoint::estimate_matrix()
{
	/* compute least squares estimate */
	dlib::matrix<float> P(1, 1);
   	dlib::matrix<float> f = dlib::zeros_matrix<float>(8,1);
	dlib::matrix<float> rhs = dlib::ones_matrix<float>(this->matches->size(),1);
	rhs *= -1.0f;
	
	P = dlib::zeros_matrix<float>(this->matches->size(),8);
	for(std::size_t i = 0; i < this->matches->size(); ++i)
	{
		P(i,0) = this->matches->at(i).first[0] * this->matches->at(i).second[0];
		P(i,1) = this->matches->at(i).first[0] * this->matches->at(i).second[1];
		P(i,2) = this->matches->at(i).first[0] 								   ;
		P(i,3) = this->matches->at(i).first[1] * this->matches->at(i).second[0];
		P(i,4) = this->matches->at(i).first[1] * this->matches->at(i).second[0];
		P(i,5) = this->matches->at(i).first[1] 								   ;
		P(i,6) = 								 this->matches->at(i).second[0];
		P(i,7) = 								 this->matches->at(i).second[1];
 	}

	dlib::qr_decomposition<dlib::matrix<float> > decomp(P);
    f = decomp.solve(rhs);
	dlib::matrix<float> F(3,3);
	for(std::size_t i = 0; i < 8; ++i)
		F(i/3,i%3) = f(i,0);
	F(2,2) = 1;
	// std::cout << F << std::endl;
	
	/* enforce rank 2 */
	dlib::matrix<float> S;
    dlib::matrix<float> U;
    dlib::matrix<float> V;
    dlib::svd3(F,U,S,V);

	/* sort eigenvalues ... */
	for(std::size_t i = 0; i < 3; ++i)
	{
		for(std::size_t j = i+1; j < 3; ++j)
		{
			if(S(i,0) < S(j,0))
			{
				std::swap(S(i,0),S(j,0));
				for(std::size_t k = 0; k < 3; ++k)
				{
					std::swap(U(k,i),U(k,j));
					std::swap(V(k,i),V(k,j));
				}
			}
		}
	}
    dlib::matrix<float> SS = dlib::identity_matrix<float>(3);
	SS(0,0) = S(0,0); SS(1,1) = S(1,0); SS(2,2) = 0.0f;
	// std::cout << U << std::endl;
	
	/* create cross product operator */
	dlib::matrix<float> e(3,1);
	for(std::size_t i = 0; i < 3; ++i)
		e(i,0) = U(2,i);
	
	dlib::matrix<float> ne = dlib::zeros_matrix<float>(3,3);
	ne(1,0) = e(2,0);
	ne(2,0) = -e(1,0);
	ne(0,1) = -e(2,0);
	ne(2,1) = e(0,0);
	ne(0,2) = e(1,0);
	ne(1,2) = -e(0,0);
	
	dlib::matrix<float> H = ne*F;
	
	F = U*SS*dlib::trans(V);
	std::cout << F << std::endl;
	
	std::cout << H << std::endl;
	
}

/* -------------------------------------------------------------- */